/** ###################################################################
**     Filename  : Modul_libely.c
**     Project   : Modul_libely
**     Processor : MC9S12XDT512BCFU
**     Version   : Driver 01.14
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 13. 8. 2012, 8:59
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/
/* MODULE Modul_libely */

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "LED1.h"
#include "LED0.h"
#include "TI1.h"
#include "AS1.h"
#include "AS2.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"
#include <stdio.h>
#include <stdlib.h>



/* User includes (#include below this line is not maintained by Processor Expert) */

#define CANE 0x80
#define INITAK 1
#define INITRQ 1
#define SLPRQ 2
#define SLPAK 2

//#define IMU_ID 0x01
//#define TLT_ID 0x02
//#define GPS1_ID    3
//#define GPS2_ID    4
//#define GPS_ID     GPS1_ID

// Rozlisenie cisla modulu
// V pripade ST1 na stavit na 1 a v pripade ST2 nastavit na 2
#define MODUL_NR 1


typedef struct 
{
 word id;
 byte len;
 byte data[8];
} can_message_t;

byte can_recv( can_message_t *pmess);
byte can_send( can_message_t *pmess);
void can_init( void);
void tilt_init(void);
byte tilt_parsing(char* data, byte sw);
void can_tilt1_send(void);
void can_tilt2_send(void);  
void can_tilt1_nodata_send(void);
void can_tilt2_nodata_send(void); 
void AS1_SendString(char* str); 
void AS2_SendString(char* str);

//premenne vyuzivane pri parsovani dat z libiel
char st_roll[40];
char st_pitch[40];
byte dlzka = 0;
char m = 0;
char q = 0;

//premenne spracovania dat z libely 1
char libela1_buf[40];
byte libela1_j = 0;
byte libela1_spracuj = 0;
byte libela1_buf_pocet;
byte libela1_busy = 0;
//datove premenne libely 1
float libela1_roll, libela1_pitch;

//premenne spracovania dat z libely 2
char libela2_buf[40];
byte libela2_j = 0;
byte libela2_spracuj = 0;
byte libela2_buf_pocet;
byte libela2_busy = 0;
//datove premenne libely 2
float libela2_roll, libela2_pitch;


byte i, tmp;
byte index_i;
char q;
unsigned short volatile *pireg;

can_message_t can_msg_libela1;
can_message_t can_msg_libela2;
can_message_t can_msg;

float roll, pitch;

byte timer_int = 0;
byte ret;

// Flag jestli uz je modul identifikovany a pripojeny k centralni jednotce
bool isConnected = 0;
// Nahodne cislo z duvodu identifikace, vybere se pozdeji v kodu
byte randNumber = 0;
// Pridelene Node ID
byte nodeID = 100;

void main(void)
{
  /* Write your local variable definition here */
  byte cnt;
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  
  /*** End of Processor Expert internal initialization.                    ***/

  /* Write your code here */
  
  for (cnt = 1; cnt < 2; cnt++) {
       Cpu_Delay100US(50000);
      }
  
  //inicializacia libiel
  tilt_init(); 
  
  //inicializacia CAN periferie 
  can_init();
  
  // Povodne nahodne cislo. Pri dvoch moduloch sa ale toto cislo vygenerovalo rovnake.
  // Z tohoto dovodu bolo pouzite cislo odvijajuce sa od cisla modulu.
  //randNumber = (byte)( rand() % 255 );
  randNumber = 131 + MODUL_NR;
  
  // uvodne vynulovanie flagov, ktore sa nastavia po prijati jedneho paketu dat z libely
  libela1_spracuj = 0;
  libela2_spracuj = 0;
  
  for(;;){
  
    // flag casovaca a blikanie LED0
    if (timer_int == 1) {
      
      timer_int = 0;
      LED0_NegVal();
             
    }
    
    // v pripade nastavenia flagu prijmu dat z libely 1 v preruseni sa vykona parsovanie
    // a nasledne ulozenie dat do prislusnych premennych
    if (libela1_spracuj == 1) {
      
      (void) tilt_parsing(libela1_buf, 1);
      
    }
    
    // podobne v priapade libely 2
    if (libela2_spracuj == 1) {
      
      (void) tilt_parsing(libela2_buf, 2); 
           
    }
    
    //******************** Prijatie spravy po CANe ********************
    ret = can_recv( &can_msg);
    if( ret == 1)
    {  
      CAN0RFLG = 0x01;
      
      // bol prijaty dotaz od master modulu pre data
      if (can_msg.id == 1) {
        // kotrola pripojenia studenstkeho modulu
        if (isConnected) {
          
          // ak data[0] == 1 posielaju sa aktualne data
          if (can_msg.data[0] == 1) {
            can_tilt1_send();
            can_tilt2_send();
          } 
          // inak sa posiela NaN
          else {
            can_tilt1_nodata_send();
            can_tilt2_nodata_send();
          }
        }
      }
      
      // poziadavka na pridelenie Node ID
      if (can_msg.id == 3) {
        if(!isConnected)
        {
          can_msg.id = 5;                                   // identifikator pro zadost o Node ID
          can_msg.len = 8;
          can_msg.data[0] = 100;                            // Standardni nove Node ID
          can_msg.data[1] = 100;                            // Typ dat podle CANaerospace (user defined)
          can_msg.data[2] = 0;                              // Servisni kod podle CANaerospace
          can_msg.data[3] = 0;                              // cislo zpravy
          can_msg.data[4] = 4;                              // Pocet implementovanych kanalu v modulu
          can_msg.data[5] = randNumber;                     // Nahodne cislo z duvodu identifikace
          can_msg.data[6] = 0;                              // Pro dalsi vyuziti
          can_msg.data[7] = 0;                              // Pro dalsi vyuziti
          while( !can_send( &can_msg));
        }        
      }
      
      // potvrdenie pridelenia Node ID
      if (can_msg.id == 4) {
        if(!isConnected)
        {          
          if(can_msg.data[4] == randNumber)
          {
            nodeID = can_msg.data[5];
            isConnected = TRUE;
          }
        }  
      }
    }
  }

  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

// prijatie spravy y CANu
byte can_recv( can_message_t *pmess)
{
 
 if( CAN0RFLG & 0x01) 
 {
   pmess->id = CAN0RXIDR0 << 8;
   pmess->id += CAN0RXIDR1;
   if( pmess->id & 0x0010) 
   { 
     pmess->len = 0x80;
   }
   else
   {
     pmess->len = 0;
   }
  
   pmess->id = ( pmess->id >> 5) & 0x07ff;
                                                                                                    
   if( ( CAN0RXDLR & 0x0f) > 8) 
   {
     pmess->len |= 0x08;
     tmp = 8;
   }
   else	
   {
     tmp = CAN0RXDLR & 0x0f;
     pmess->len |= tmp;  
   }
   
   for( i=0; i < tmp; i++)
   {
     pmess->data[i] = (&CAN0RXDSR0)[i];
   }
   return 1;
  }
  return 0;
}

// odoslanie spravy po CANe
byte can_send( can_message_t *pmess)
{
  
 
  if( CAN0TFLG & 0x01)
  {
    pireg = ( unsigned short volatile *)( &CAN0TXIDR0);
    if( pmess->len & 0x80)
    { 
      *pireg = ( ( pmess->id << 5) & 0xffe0) | 0x0010;	// write id, rtr frame
    }
    else
    {
      *pireg = ( ( pmess->id << 5) & 0xffe0); // write id, data frame
    }
    pireg++;
    *pireg = 0;
    
    pmess->len &= 0x0f;
    for( i = 0; i < pmess->len; i++)
    {
      ( &CAN0TXDSR0)[i] = pmess->data[i];	  // write data
    }
    CAN0TXDLR = pmess->len; 	        // write dlc
    CAN0TFLG = 1;	 					  // start transmission
    return 1;
  }

  return 0;
}

// inicializacia CANu
void can_init(void)
{
  CAN0CTL0 |= INITRQ; // set initrq, this will also set initak
  while( !( CAN0CTL1 & INITAK)); // wait for init mode to occur

  CAN0CTL1 = CANE; // set cane just in case this is the first time after reset

  CAN0BTR0=0x01; // for 500kbps 0x03, for 100kbps 0x13
  CAN0BTR1=0x32; // for a can Baud of 500kbps at 16Mhz crystal

  // can id filter bits
  CAN0IDAR0=0x00;
  CAN0IDAR1=0x00;
  CAN0IDAR2=0x00;
  CAN0IDAR3=0x00;
  CAN0IDAR4=0x00;
  CAN0IDAR5=0x00;
  CAN0IDAR6=0x00;
  CAN0IDAR7=0x00;

  // can id filter mask
  CAN0IDMR0=0xff;
  CAN0IDMR1=0xff;
  CAN0IDMR2=0xff;
  CAN0IDMR3=0xff;
  CAN0IDMR4=0xff;
  CAN0IDMR5=0xff;
  CAN0IDMR6=0xff;
  CAN0IDMR7=0xff;

  CAN0IDAC = 0x10; // set filters to 2 16 bit acceptance filters

  CAN0CTL0 &= ~INITRQ; // clear initrq
  while (CAN0CTL1 & INITAK);

  CAN0CTL1 |= CANE; // set cane just in case this is the first time after reset
  CAN0RIER_RXFIE = 0; // enable receive interrupt

  CAN0TBSEL = 1;
}

// funkcia vysiela pole charov znak po znaku na seriovu linku AS1, kym nenarazi na 0x00
void AS1_SendString(char* str) {
   while (*str != 0x00) {
		  (void) AS1_SendChar((byte) *str);
      while( AS1_GetTxComplete() == 0){};
		++str;
	}
}

// funkcia vysiela pole charov znak po znaku na seriovu linku AS2, kym nenarazi na 0x00
void AS2_SendString(char* str) {
   while (*str != 0x00) {
		  (void) AS2_SendChar((byte) *str);
      while( AS2_GetTxComplete() == 0){};
		++str;
	}
}

// inicializacia libiel
void tilt_init(void) {
  AS1_SendString("h\r\n");
  AS2_SendString("h\r\n");
  Cpu_Delay100US(10000);
  AS1_SendString("ave=1\r\n");
  AS2_SendString("ave=1\r\n");
  Cpu_Delay100US(200);
  AS1_SendString("save\r\n");
  AS2_SendString("save\r\n");
  Cpu_Delay100US(200);
  AS1_SendString("period=1\r\n");
  AS2_SendString("period=1\r\n");
  Cpu_Delay100US(200);
  AS1_SendString("save\r\n");
  AS2_SendString("save\r\n");
  Cpu_Delay100US(200);
  AS1_SendString("go\r\n");
  AS2_SendString("go\r\n");
  Cpu_Delay100US(10000);  
}

// parsovanie pola znakov prijateho od libely
// typicky vystup z libely AOSI EZ-TILT:
// "R  -1.16         P  0.32        "
byte tilt_parsing(char* data, byte sw) {
  
  byte parsed = 0;
  m = 0;
  q = 1;
  
  // kontrola ci je prvy znak 'R' a druhy medzera      
  if (*data == 'R' && *(data + 1) == ' ') {
     LED1_NegVal(); 
     ++data;
     
     // preskocenie medzier               
     while (*data == ' ' && q < 40) {
       ++q;
       ++data;
     }
     
     // vynulovanie pola
     for (m = 0; m < 40; ++m) {
        st_roll[m] = 0x00;
     }
     
     // nacitanie cisla kym sa nenarazi na medzeru
     m = 0;
     while (*data != ' ' && q < 40) {
       st_roll[m] = *data;
       ++q;
       ++m;
       ++data;  
     }
     
     // prevod nacitaneho cisla do floatu a ulozenie do prislusnej premennej
     // podla toho, o ktoru libelu sa jedna
     if (sw == 1) {
        parsed += sscanf(st_roll, "%f", &libela1_roll);
     }
     else if (sw == 2) {
        parsed += sscanf(st_roll, "%f", &libela2_roll);
     }
     
     // posun na pismeno 'P'
     while (*data != 'P' && q < 40) {
       ++q;
       ++data;
     }
     
     ++q;
     ++data;
     
     // preskocenie medzier
     while (*data == ' ' && q < 40) {
       ++q;
       ++data;
     }
     
     // vynulovanie pola
     for (m = 0; m < 40; ++m) {
        st_pitch[m] = 0x00;
     }
     
     // nacitanie cisla kym sa nenarazi na medzeru
     m = 0;
     while (*data != ' ' && q < 40) {
       st_pitch[m] = *data;
       ++q;
       ++m;
       ++data;  
     }
     
     // prevod nacitaneho cisla do floatu a ulozenie do prislusnej premennej
     // podla toho, o ktoru libelu sa jedna
     if (sw == 1) {
      parsed += sscanf(st_pitch, "%f", &libela1_pitch);
     } 
     else if (sw == 2) {
      parsed += sscanf(st_pitch, "%f", &libela2_pitch);    
     }
  }
  
  // nulovanie prislusnych flagov
  if (sw == 1) {
    libela1_spracuj = 0;
    libela1_j = 0;
    libela1_busy = 0;
  }
  else if (sw == 2) {
    libela2_spracuj = 0;
    libela2_j = 0;
    libela2_busy = 0;
  }
  
  return parsed;
}

// funkcia odosle po CANe spravu s aktualnymi hodnotami
// naklonu a sklonu nameraneho libelou 1
void can_tilt1_send(void) {

  can_msg_libela1.id = 0x137;
  can_msg_libela1.len = 8; 
  can_msg_libela1.data[0] = nodeID;
  can_msg_libela1.data[1] = 2;
  can_msg_libela1.data[2] = 0;
  can_msg_libela1.data[3] = 1;     // relativni pozicia studentskych
  can_msg_libela1.data[4] = ( ( unsigned char*) &libela1_pitch)[0];
  can_msg_libela1.data[5] = ( ( unsigned char*) &libela1_pitch)[1];
  can_msg_libela1.data[6] = ( ( unsigned char*) &libela1_pitch)[2];
  can_msg_libela1.data[7] = ( ( unsigned char*) &libela1_pitch)[3];
  while( !can_send( &can_msg_libela1));

  can_msg_libela1.id = 0x138;
  can_msg_libela1.len = 8; 
  can_msg_libela1.data[0] = nodeID;
  can_msg_libela1.data[1] = 2;
  can_msg_libela1.data[2] = 0;
  can_msg_libela1.data[3] = 2;
  can_msg_libela1.data[4] = ( ( unsigned char*) &libela1_roll)[0];
  can_msg_libela1.data[5] = ( ( unsigned char*) &libela1_roll)[1];
  can_msg_libela1.data[6] = ( ( unsigned char*) &libela1_roll)[2];
  can_msg_libela1.data[7] = ( ( unsigned char*) &libela1_roll)[3];
  while( !can_send( &can_msg_libela1));
          
}

// funkcia odosle po CANe spravu s aktualnymi hodnotami
// naklonu a sklonu nameraneho libelou 2
void can_tilt2_send(void) {
  
  can_msg_libela2.id = 0x137;
  can_msg_libela2.len = 8; 
  can_msg_libela2.data[0] = nodeID;
  can_msg_libela2.data[1] = 2;
  can_msg_libela2.data[2] = 0;
  can_msg_libela2.data[3] = 3;
  can_msg_libela2.data[4] = ( ( unsigned char*) &libela2_pitch)[0];
  can_msg_libela2.data[5] = ( ( unsigned char*) &libela2_pitch)[1];
  can_msg_libela2.data[6] = ( ( unsigned char*) &libela2_pitch)[2];
  can_msg_libela2.data[7] = ( ( unsigned char*) &libela2_pitch)[3];
  while( !can_send( &can_msg_libela2));

  can_msg_libela2.id = 0x138;
  can_msg_libela2.len = 8; 
  can_msg_libela2.data[0] = nodeID;
  can_msg_libela2.data[1] = 2;
  can_msg_libela2.data[2] = 0;
  can_msg_libela2.data[3] = 4;
  can_msg_libela2.data[4] = ( ( unsigned char*) &libela2_roll)[0];
  can_msg_libela2.data[5] = ( ( unsigned char*) &libela2_roll)[1];
  can_msg_libela2.data[6] = ( ( unsigned char*) &libela2_roll)[2];
  can_msg_libela2.data[7] = ( ( unsigned char*) &libela2_roll)[3];
  while( !can_send( &can_msg_libela2)); 
 }

// funkcia odosle po CANe spravu s NaN hodnotami
// naklonu a sklonu libely 1
void can_tilt1_nodata_send(void) {
  can_msg_libela1.id = 0x137;
  can_msg_libela1.len = 4;
  can_msg_libela1.data[0] = nodeID;
  can_msg_libela1.data[1] = 2;
  can_msg_libela1.data[2] = 0;
  can_msg_libela1.data[3] = 1;
 /* can_msg_libela1.data[4] = 0xFF;
  can_msg_libela1.data[5] = 0xFF;
  can_msg_libela1.data[6] = 0x00;
  can_msg_libela1.data[7] = 0x00;        */
  while( !can_send( &can_msg_libela1));

  can_msg_libela1.id = 0x138;
  can_msg_libela1.len = 4;
  can_msg_libela1.data[0] = nodeID;
  can_msg_libela1.data[1] = 2;
  can_msg_libela1.data[2] = 0;
  can_msg_libela1.data[3] = 2;
/*  can_msg_libela1.data[4] = 0xFF;
  can_msg_libela1.data[5] = 0xFF;
  can_msg_libela1.data[6] = 0x00;
  can_msg_libela1.data[7] = 0x00;  */
  while( !can_send( &can_msg_libela1));        
}

// funkcia odosle po CANe spravu s NaN hodnotami
// naklonu a sklonu libely 2
void can_tilt2_nodata_send(void) {
  can_msg_libela2.id = 0x137;
  can_msg_libela2.len = 4;
  can_msg_libela2.data[0] = nodeID;
  can_msg_libela2.data[1] = 2;
  can_msg_libela2.data[2] = 0;
  can_msg_libela2.data[3] = 3;
/*  can_msg_libela2.data[4] = 0xFF;
  can_msg_libela2.data[5] = 0xFF;
  can_msg_libela2.data[6] = 0x00;
  can_msg_libela2.data[7] = 0x00;          */
  while( !can_send( &can_msg_libela2));

  can_msg_libela2.id = 0x138;
  can_msg_libela2.len = 4;
  can_msg_libela2.data[0] = nodeID;
  can_msg_libela2.data[1] = 2;
  can_msg_libela2.data[2] = 0;
  can_msg_libela2.data[3] = 4;
/*  can_msg_libela2.data[4] = 0xFF;
  can_msg_libela2.data[5] = 0xFF;
  can_msg_libela2.data[6] = 0x00;
  can_msg_libela2.data[7] = 0x00;        */
  while( !can_send( &can_msg_libela2));
}


/* END Modul_libely */
/*
** ###################################################################
**
**     This file was created by Processor Expert 3.02 [04.44]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
